/* MS1 FSM Test 0.1
 *
 * needs following setup:
 * modprobe can
 * modprobe can-raw
 * modprobe vcan
 * ip link add type vcan
 * ifconfig vcan0 up
 */
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <libgen.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <netinet/in.h>

#include <linux/can.h>
#include "maerklin.h"

int get_wert_for_index (int index) {
    switch (index) {
       case 1:
            return(0x00); /* GO */
            break;
        case 2:
            return(0x00); /* kein Kurzschluss */
            break;
        case 3:
            return(0x00);
            break;
        case 4:
            return(0x01); /* Normalbetrieb */
            break;
        default:
            break;
    }
    return 0;
}

void printf_bin(int zahl) {
    int i;
    for(i=(sizeof(zahl) * 8) - 1; i >= 0;i--) {
        printf("%d",(zahl & (0x01 << i)) > 0);
    }
}

void print_uid(uint8_t *p) {
    int i;
    printf("stored MS1 UID: ");
    for (i=0;i <4 ; i++) {
        printf("0x%02x ", p[i]);
    }
    printf("\n");
}

void get_handle(struct ms1_handle *handle, uint32_t id) {
    bzero(handle,sizeof(struct ms1_handle));
    handle->node = (id & MS1_FRAME_NODE_MASK) >> MS1_FRAME_NODE_SHIFT;
    handle->prio = (id & MS1_FRAME_PRIO_MASK) >> MS1_FRAME_PRIO_SHIFT;
    if (handle->prio == 7) {
        handle->direction = (id & MS1_FRAME_DIRECTION_MASK) >> MS1_FRAME_DIRECTION_SHIFT;
        handle->MID       = (id & MS1_FRAME_MID_MASK)       >> MS1_FRAME_MID_SHIFT;
        handle->stage     = (id & MS1_FRAME_STAGE_MASK)     >> MS1_FRAME_STAGE_SHIFT;
        handle->UID       = (id & MS1_FRAME_UID_MASK)       >> MS1_FRAME_UID_SHIFT;
    } else {
        handle->command   = (id & MS1_FRAME_COMMAND_MASK)   >> MS1_FRAME_COMMAND_SHIFT;
        handle->object    = (id & MS1_FRAME_OBJECT_MASK)    >> MS1_FRAME_OBJECT_SHIFT;
    }
}

void print_ms1_handle(struct ms1_handle *handle) {
    printf("MS1 handle    prio=0x%02x\n", handle->prio);
    if (handle->prio == 7) {
       printf("MS1 handle     UID=0x%02x\n", handle->UID);
       printf("MS1 handle   stage=0x%02x\n", handle->stage);
       printf("MS1 handle     MID=0x%02x\n", handle->MID);
       printf("MS1 handle     dir=0x%02x\n", handle->direction);
       printf("MS1 handle    node=0x%02x\n", handle->node);
    } else {
       printf("MS1 handle  object=0x%04x\n", handle->object);
       printf("MS1 handle command=0x%02x\n", handle->command);
       printf("MS1 handle    node=0x%02x\n", handle->node);
    }
}

void to_can(uint32_t canid, int dlc , uint8_t *data, int sc) {
    int nbytes;
    struct can_frame frame;
    frame.can_id  = canid;
    frame.can_id &= CAN_EFF_MASK;
    frame.can_id |= CAN_EFF_FLAG;
    frame.can_dlc = dlc;
    memcpy(&frame.data,data,dlc);
    if ((nbytes = write(sc, &frame, sizeof(frame))) != sizeof(frame)) {
        perror("CAN write error");
    }
}

void print_can_frame(struct can_frame *can_frame) {
    uint8_t i;
    printf("CAN 0x%08X",can_frame->can_id);
    printf(" [%d]", can_frame->can_dlc);
    for (i=0; i<can_frame->can_dlc;i++) {
        printf(" %02x", can_frame->data[i]);
    }
    printf("\n");
}

int init_can(char *can_interface) {
    int sc;
    struct ifreq ifr;
    struct sockaddr_can caddr;

    socklen_t caddrlen = sizeof(caddr);
    strcpy(ifr.ifr_name, can_interface);

    if ((sc = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("Can't connect to can socket \n");
        exit(1);
    }
    caddr.can_family = AF_CAN;
    if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
        perror("SIOCGIFINDEX");
        exit(1);
    }
    caddr.can_ifindex = ifr.ifr_ifindex;

    if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
       perror("bind error");
       exit(1);
    }
    printf("CAN Socket %s opened\n",can_interface);
    return sc;
}

int process_cs2_can_frame(struct can_frame *can_frame) {
    return 0;
}

/* create entry for new MS1 - recursive */

struct ms1_id *add_ms1_id(struct ms1_id *pms1_id, uint32_t Master_UID, uint32_t Slave_UID) {
    /* create a new MS1 tree node entry if necessary */
    if (pms1_id == NULL) {
        pms1_id = (struct ms1_id *) malloc (sizeof(struct ms1_id));
        bzero(pms1_id,sizeof(struct ms1_id));
        pms1_id->Master_UID=Master_UID;
        pms1_id->Slave_UID=Slave_UID;
        pms1_id->left=NULL;
        pms1_id->right=NULL;
    } else if (Slave_UID < pms1_id->Slave_UID ) {
        pms1_id->left  = add_ms1_id(pms1_id->left,  Master_UID, Slave_UID);
    } else {
        pms1_id->right = add_ms1_id(pms1_id->right, Master_UID, Slave_UID);
    }
    return pms1_id;
}

/* print MS1 entries - recursive */

void tprint_ms1_ids(struct ms1_id *pms1_id) {
   if (pms1_id != NULL) {
       tprint_ms1_ids(pms1_id->left);
       printf("Master UID : 0x%08x Slave UID 0x%08x state:%d\n", pms1_id->Master_UID,  pms1_id->Slave_UID, pms1_id->state);
       tprint_ms1_ids(pms1_id->right);
   }
}

int process_ms1_can_frame(struct can_frame *can_frame, struct ms1_handle *my_ms1_handle,
     uint8_t *ms1_stage, int sc) {

    uint32_t i;

    get_handle(my_ms1_handle,can_frame->can_id);
    print_ms1_handle(my_ms1_handle);


    switch(my_ms1_handle->prio) {

        case 0x07: /* Anmeldung */

            if ( my_ms1_anmeldung_state[*ms1_stage].switch_state == my_ms1_handle->stage) {
                printf("State %d -> %d\n", *ms1_stage, my_ms1_anmeldung_state[*ms1_stage].next_state);
                if (my_ms1_anmeldung_state[*ms1_stage].can_message) {
                    can_frame->can_id ^= 0x00000080U;
                    to_can(can_frame->can_id, 8, my_ms1_anmeldung_state[*ms1_stage].can_message,sc);
                }
                /* copying UID to message and store the UID */
                if (my_ms1_anmeldung_state[*ms1_stage].uid_oct) {
                    *my_ms1_anmeldung_state[*ms1_stage].uid_oct = my_ms1_handle->UID;
                }
                printf("%s\n",my_ms1_anmeldung_state[*ms1_stage].debug_message);
                /* switch to next stage */
                *ms1_stage = my_ms1_anmeldung_state[*ms1_stage].next_state;
                print_uid(&ms1_stage7_msg[0]);
            }
            break;

        case 0x03: /* Ping Pong (Zyklische Ueberwachung) */
            /* copying UID to message */
            for (i=4;i<=7;i++) {
                ms1_ping1_msg[i]=ms1_stage7_msg[i-4];
            }

            can_frame->can_id -= 1;
            to_can(can_frame->can_id, 8, ms1_ping1_msg,sc);

            break;

        case 0x06: /* Aufruestung */
            if ( my_ms1_aufruestung_state[*ms1_stage].switch_state == my_ms1_handle->command) {
                printf("State %d -> %d\n", *ms1_stage, my_ms1_aufruestung_state[*ms1_stage].next_state);
                if (my_ms1_aufruestung_state[*ms1_stage].can_message) {
                    can_frame->can_id -= 1;
                    to_can(can_frame->can_id, 8, my_ms1_aufruestung_state[*ms1_stage].can_message,sc);
                }

                printf("%s\n",my_ms1_aufruestung_state[*ms1_stage].debug_message);
                /* switch to next stage */
                *ms1_stage = my_ms1_aufruestung_state[*ms1_stage].next_state;
            }

            break;
    }
    return 0;
}



int main(int argc, char **argv) {
    int nbytes,sc,ret;
    fd_set readfds;
    struct can_frame *can_frame;
    struct ms1_handle *my_ms1_handle;
    uint8_t ms1_stage=MS1_START;
    uint32_t Master_UID=0x01010101U;
    struct ms1_id *rootMS1_ID;
    rootMS1_ID=NULL;

    sc=init_can("vcan0");
    my_ms1_handle = (struct ms1_handle *)malloc(sizeof(struct ms1_handle));
    can_frame = (struct can_frame *)malloc(sizeof(struct can_frame));

    while (1) {
        FD_ZERO(&readfds);
        FD_SET(sc, &readfds);
        ret = select(sc+1, &readfds, NULL, NULL, NULL);
        printf("select returns %d\n",ret);
        /* received a CAN frame */
        if (FD_ISSET(sc, &readfds)) {
            if ((nbytes = read(sc, can_frame, sizeof(struct can_frame))) < 0) {
                perror("read error on CAN\n");
            }
            can_frame->can_id &= CAN_EFF_MASK;
            print_can_frame(can_frame);
            /* check whether we have a MS1/CS1 or a MS2/CS2 CAN frame */
            if ((can_frame->can_id & M_CS2_HASH_MASK) == M_CS2_HASH_FLAG) {
                /* this is a MS2/CS2 CAN frame */
                process_cs2_can_frame(can_frame);
            } else {
                /* this is a MS1/CS1 CAN frame */
                process_ms1_can_frame(can_frame, my_ms1_handle, &ms1_stage, sc);
            }
        }
    }
    return 0;
}
